#include "ti_msp_dl_config.h"

void set_Motor(int16_t left, int16_t right)
{
    // LEFT motor control
    if (left > 0)
    {
        DL_GPIO_setPins(GPIO_MOTOR_AIN_1_PORT, GPIO_MOTOR_AIN_1_PIN);
        DL_GPIO_clearPins(GPIO_MOTOR_AIN_2_PORT, GPIO_MOTOR_AIN_2_PIN);
    }
    else if (left < 0)
    {
        DL_GPIO_clearPins(GPIO_MOTOR_AIN_1_PORT, GPIO_MOTOR_AIN_1_PIN);
        DL_GPIO_setPins(GPIO_MOTOR_AIN_2_PORT, GPIO_MOTOR_AIN_2_PIN);
        left=-left;
    }
    else // left == 0
    {
        DL_GPIO_setPins(GPIO_MOTOR_AIN_1_PORT, GPIO_MOTOR_AIN_1_PIN);
        DL_GPIO_setPins(GPIO_MOTOR_AIN_2_PORT, GPIO_MOTOR_AIN_2_PIN); // 刹车或停转
    }

    // RIGHT motor control
    if (right > 0)
    {
        DL_GPIO_setPins(GPIO_MOTOR_BIN_1_PORT, GPIO_MOTOR_BIN_1_PIN);
        DL_GPIO_clearPins(GPIO_MOTOR_BIN_2_PORT, GPIO_MOTOR_BIN_2_PIN);
    }
    else if (right < 0)
    {
        DL_GPIO_clearPins(GPIO_MOTOR_BIN_1_PORT, GPIO_MOTOR_BIN_1_PIN);
        DL_GPIO_setPins(GPIO_MOTOR_BIN_2_PORT, GPIO_MOTOR_BIN_2_PIN);
        right=-right;
    }
    else // right == 0
    {
        DL_GPIO_setPins(GPIO_MOTOR_BIN_1_PORT, GPIO_MOTOR_BIN_1_PIN);
        DL_GPIO_setPins(GPIO_MOTOR_BIN_2_PORT, GPIO_MOTOR_BIN_2_PIN); // 刹车或停转
    }

    if(left>2000)
    {
        left=2000;
    }
    else
    {
        left = left;
    }
    if(right>2000)
    {
        right=2000;
    }
    else
    {
        right = right;
    }

    DL_TimerG_setCaptureCompareValue(MOTOR_PWM_INST,left,DL_TIMER_CC_0_INDEX);
    DL_TimerG_setCaptureCompareValue(MOTOR_PWM_INST,right,DL_TIMER_CC_1_INDEX);
}
